import sys
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from customized_msgs.msg import FeedBack, ArgSetLength
from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QPushButton, QLabel
from PyQt5.QtGui import QKeySequence, QFont
from PyQt5.QtCore import QSize, QTimer
from threading import Thread

def spin_thread(node):
    rclpy.spin(node)

class SimpleGuiNode(Node):
    def __init__(self):
        super().__init__('simple_gui_node')

        self.current_value = 0.0
        self.motor1_feedback = {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0}
        self.motor2_feedback = {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0}
        self.timer_active = False  # 用于跟踪定时器状态

        # 创建发布者
        self.publisher_ = self.create_publisher(Float32, '/gcs_client_winch/virtual_rc', 10)
        self.arg_set_length_publisher_ = self.create_publisher(ArgSetLength, '/winch_wireless_server/set_length', 10)

        # 创建订阅者
        self.motor1_subscriber_ = self.create_subscription(
            FeedBack,
            '/motor_feedback/motor1_winch',
            self.motor1_feedback_callback,
            10
        )

        self.motor2_subscriber_ = self.create_subscription(
            FeedBack,
            '/motor_feedback/motor2_winch',
            self.motor2_feedback_callback,
            10
        )


        # 线程启动
        self.spin_thread_ = Thread(target=spin_thread, args=(self,))
        self.spin_thread_.start()

        # 创建Qt应用和窗口
        self.app = QApplication(sys.argv)
        self.window = QWidget()
        self.window.setWindowTitle('ROS 2 GUI')
        self.window.resize(600, 400)

        self.layout = QVBoxLayout()

        # 设置字体大小
        font = QFont('Arial', 16)
        self.app.setFont(font)

        # 显示当前值的标签
        self.value_label = QLabel(f'Current Value: {self.current_value:.1f}')
        self.layout.addWidget(self.value_label)

        # 添加更新用的 text_label
        self.text_label = QLabel(self.get_display_text())
        self.layout.addWidget(self.text_label)

        # 添加按钮和绑定事件
        self.add_button('Button A (-0.2)', 'A', self.on_button_a)
        self.add_button('Button S (0)', 'S', self.on_button_s)
        self.add_button('Button D (+0.2)', 'D', self.on_button_d)
        self.add_button('Toggle ArgSetLength', 'T', self.on_toggle_timer)  # 新增按钮
        
        self.window.setLayout(self.layout)
        self.window.show()

        
        # 使用 ROS 2 的计时器代替 QTimer
        self.create_timer(0.1, self.update_display)  # 10Hz 频率
        self.timer = self.create_timer(25.0, self.on_timer_timeout)  # 20秒周期

    def get_display_text(self):
        return (f'Current Value: {self.current_value:.1f}\n\n'
                f'Motor 1 Feedback:\n'
                f'Angle: {self.motor1_feedback["angle"]:.2f}\n'
                f'Velocity: {self.motor1_feedback["velocity"]:.2f}\n'
                f'Torque: {self.motor1_feedback["torque"]:.2f}\n\n'
                f'Motor 2 Feedback:\n'
                f'Angle: {self.motor2_feedback["angle"]:.2f}\n'
                f'Velocity: {self.motor2_feedback["velocity"]:.2f}\n'
                f'Torque: {self.motor2_feedback["torque"]:.2f}')

    def update_display(self):
        self.text_label.setText(self.get_display_text())

    def add_button(self, label, shortcut, callback):
        button = QPushButton(label)
        button.setShortcut(QKeySequence(shortcut))
        button.clicked.connect(callback)
        button.setMinimumHeight(50)  # 设置按钮高度
        self.layout.addWidget(button)

    def publish_value(self):
        # 限制当前值在-1和1之间
        self.current_value = max(-1.0, min(1.0, self.current_value))
        msg = Float32()
        msg.data = self.current_value
        self.publisher_.publish(msg)
        self.get_logger().info(f'Published: {self.current_value}')

    def on_button_a(self):
        self.current_value -= 0.2
        self.publish_value()

    def on_button_s(self):
        self.current_value = 0.0
        self.publish_value()

    def on_button_d(self):
        self.current_value += 0.2
        self.publish_value()

    def on_toggle_timer(self):
        if self.timer_active:
            self.get_logger().info('Stopped ArgSetLength publishing.')
        else:
            self.last_pub = False
            self.pub_arg_set_length(True)
            self.get_logger().info('Started ArgSetLength publishing.')
        self.timer_active = not self.timer_active

    def pub_arg_set_length(self, target):
        msg = ArgSetLength()
        msg.abs_ctrl = True
        if target:
            msg.length = 0.5
        else:
            # msg.length = 4.0
            msg.length = 18.0
        self.arg_set_length_publisher_.publish(msg)

    def on_timer_timeout(self):
        if self.timer_active == False:
            return
        self.pub_arg_set_length(self.last_pub)
        self.last_pub = not self.last_pub

    def motor1_feedback_callback(self, msg):
        self.motor1_feedback['angle'] = msg.feedback_angle
        self.motor1_feedback['velocity'] = msg.feedback_velocity
        self.motor1_feedback['torque'] = msg.feedback_torque

    def motor2_feedback_callback(self, msg):
        self.motor2_feedback['angle'] = msg.feedback_angle
        self.motor2_feedback['velocity'] = msg.feedback_velocity
        self.motor2_feedback['torque'] = msg.feedback_torque

    def run(self):
        # 启动Qt应用事件循环
        sys.exit(self.app.exec_())

def main(args=None):
    rclpy.init(args=args)
    node = SimpleGuiNode()
    
    try:
        node.run()
    except KeyboardInterrupt:
        node.get_logger().info('Shutting down node...')
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()
